#include "encode.h"

uint8_t G711Encoder::encodeMuLaw(int16_t pcm_val) {
    const uint16_t BIAS = 0x84;
    const uint16_t CLIP = 32635;
    uint8_t sign, exponent, mantissa;
    uint16_t magnitude;

    sign = (pcm_val < 0) ? 0x80 : 0;
    if (sign != 0) pcm_val = -pcm_val;
    pcm_val = pcm_val > CLIP ? CLIP : pcm_val;
    pcm_val += BIAS;
    magnitude = (pcm_val > 0xFF) ? pcm_val >> 1 : pcm_val;

    exponent = 7;
    for (uint16_t exp_mask = 0x4000; (magnitude & exp_mask) == 0 && exponent > 0; exp_mask >>= 1) {
        exponent--;
    }

    mantissa = (magnitude >> (exponent + 3)) & 0x0F;
    return ~(sign | (exponent << 4) | mantissa);
}

uint8_t G711Encoder::encodeALaw(int16_t pcm_val) {
    const uint16_t BIAS = 0x84;
    const uint16_t CLIP = 32635;
    uint8_t sign, exponent, mantissa;
    uint16_t magnitude;

    sign = (pcm_val < 0) ? 0x00 : 0x80;
    if (sign != 0) pcm_val = -pcm_val;
    pcm_val = pcm_val > CLIP ? CLIP : pcm_val;
    pcm_val += BIAS;
    magnitude = (pcm_val > 0xFF) ? pcm_val >> 1 : pcm_val;

    exponent = 7;
    for (uint16_t exp_mask = 0x4000; (magnitude & exp_mask) == 0 && exponent > 0; exp_mask >>= 1) {
        exponent--;
    }

    mantissa = (magnitude >> (exponent + 3)) & 0x0F;
    return (sign | (exponent << 4) | mantissa) ^ 0x55;
}
